#include <Servo.h>

// 无刷电机控制
Servo Motor1;
Servo Motor2;
// 舵机控制
Servo Servo1;
Servo Servo2;
// 接收无线数传数据
String command = "";

void setup() {
  // --- 无刷电机 ---
  Motor1.attach(9, 1000, 2000);
  Motor2.attach(10, 1000, 2000);
  // --- 无线数传 ---
  Serial.begin(9600);
  Serial.println("Serial monitor starts successfully!");
}

void loop() {
  // 接收数传信息
  while (Serial.available() > 0) {
    command += char(Serial.read());
    delay(2);
  }
  // 命令解析
  int q_placeinstring = -1;
  int ml_placeinstring = -1;
  int mr_placeinstring = -1;
  int m_placeinstring = -1;
  int sl_placeinstring = -1;
  int sr_placeinstring = -1;
  int s_placeinstring = -1;

  String speed_string;
  String angle_string;
  int speed_int;
  int angle_int;
  if (command.length() > 0) {
    Serial.println(command); // 显示接收到的命令
    q_placeinstring =command.indexOf("q");
    ml_placeinstring =command.indexOf("ml");
    mr_placeinstring =command.indexOf("mr");
    sl_placeinstring =command.indexOf("sl");
    sr_placeinstring =command.indexOf("sr");
  }
  
  if (q_placeinstring != -1)
  { Motor1.write(1500); Motor2.write(1500); Servo1.write(0); Servo2.write(2);}
  if (ml_placeinstring != -1) {
    speed_string = command.substring(ml_placeinstring + 1);
    speed_int = speed_string.toInt();
    Motor1.write(speed_int);
    Serial.print("the left motor's speed has successfully been adjusted to");
    Serial.println(speed_string);
  }
  if (mr_placeinstring != -1) {
    speed_string = command.substring(mr_placeinstring + 1);
    speed_int = speed_string.toInt();
    Motor1.write(speed_int);
    Serial.print("the right motor's speed has successfully been adjusted to");
    Serial.println(speed_string);
  }
  if (sl_placeinstring != -1) {
    angle_string = command.substring(sl_placeinstring + 1);
    angle_int = angle_string.toInt();
    Motor1.write(angle_int);
    Serial.print("the left servo's speed has successfully been adjusted to");
    Serial.println(angle_string);
  }
  if (sr_placeinstring != -1) {
    angle_string = command.substring(sr_placeinstring + 1);
    angle_int = angle_string.toInt();
    Motor1.write(angle_int);
    Serial.print("the right servo's speed has successfully been adjusted to");
    Serial.println(angle_string);
  }


  if(command)
  command = ""; // 命令清空
}
